/*
 *
 * Copyright (C) 2014
 * Julio Jarquin, Gustavo Arechavaleta <garechav@cinvestav.edu.mx>
 * CINVESTAV - Saltillo Campus
 *
 * This file is part of HRLocomotion-1.0
 * HRLocomotion-1.0 free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * HRLocomotion-1.0 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301  USA
 */

/**
 *	\file src/mpc_data.cc
 *	\author Julio Jarquin, Gustavo Arechavaleta
 *	\version 1.0
 *	\date 2015
 *
 *	Implementation of the MPCData class.
 */

#include "HRLocomotion/mpc_data.h"
#include <iostream>
#include <boost/shared_ptr.hpp>


namespace hrLocomotion{
void MPCData::init(){
    initHessian();
    pk.resize(2*(N_horizon+m_steps));
    pk.setZero();
}

MPCData& MPCData::operator =(const MPCData & other){
    /*
    g = other.g;      // Gravity constant
    h_com = other.h_com;      /* CoM fixed heigh
    alpha = other.alpha;         /* Jerk gain
    beta = other.beta;         /* Velocity gain
    gamma = other.gamma;        /* ZMP gain
    T_s = other.T_s;           /* Double support period

    m_steps = other.m_steps;      /* Future m steps to be considered
    s_stage = other.s_stage;     /* Duration of single support stage in Periods T_s. Double support stage < T_s
    N_horizon = other.N_horizon;      /* Previewable window in periods T_s

    stage = other.stage; /*Current stage for the varying matrix Q

    //foot = other.foot;         /* Current foot on ground , kLeftFoot or kRightFoot
*/
    return *this;
}



void MPCData::initMatricesP(){
    using std::pow;

    // setting Pvs
    Pvs.resize(N_horizon,3);
    for(int i=0; i!=N_horizon; i++){
        Pvs.row(i) << 0, 1, (i+1)*T_s;
    }

    // setting Pvu
    Pvu.setZero(N_horizon,N_horizon);
    VectorXr tempPvu(N_horizon,1);
    for(int i=0; i!=N_horizon; i++){
        tempPvu(i) = (1+2*i)*pow(T_s,2)/2;
    }
    for(int i=N_horizon, j=0; i>0; i--, j++){
        Pvu.col(j).tail(i) = tempPvu.head(i);
    }

    // setting Pzs
    Pzs.resize(N_horizon,3);
    for(int i=0; i!=N_horizon; i++){
        Pzs.row(i) << 1, (i+1)*T_s, (pow((double)(i+1),2)*pow(T_s,2)/2)-(h_com/g);
    }

    // setting Pzu
    Pzu.setZero(N_horizon,N_horizon);
    VectorXr tempPzu(N_horizon,1);
    for(int i=0; i!=N_horizon; i++){
        tempPzu(i) = ((1+3*i+3*pow((double)i,2))*pow(T_s,3)/6)-(T_s*h_com/g);
    }
    for(int i=N_horizon, j=0; i>0; i--, j++){
        Pzu.col(j).tail(i) = tempPzu.head(i);
    }

}

void MPCData::initMatricesU(){
    // setting Uc

    VectorXr tempUc = VectorXr::Zero(N_horizon,1);
    for(int i=0; i!=s_stage; i++){
        Uc.push_back(tempUc);
    }
    for(int i=0, j=s_stage-1; i<s_stage-1; i++, j--){
        Uc[i].head(j).setConstant(1);
    }

    // setting Ur
    MatrixXr tempUr = MatrixXr::Zero(N_horizon,m_steps);
    for(int i=0; i!=s_stage; i++){
        Ur.push_back(tempUr);
    }

    for(int i=0; i!=s_stage; i++){
        int col=0;
        int con=0;
        for(int j=s_stage-1-i; j<N_horizon; j++){
            Ur[i](j,col) = 1;
            con++;
            if(con==8){
                col++;
                con = 0;
            }
        }
    }
}

void MPCData::initHessian(){

    initMatricesP();
    initMatricesU();

    MatrixXr Qtes(N_horizon+m_steps,N_horizon+m_steps), blockzero(N_horizon+m_steps,N_horizon+m_steps), eye(N_horizon,N_horizon);
    //Qaux(2*(N_horizon+m_steps),2*(N_horizon+m_steps)) //without shared_ptr
    boost::shared_ptr<MatrixXr> Qaux ( new MatrixXr (2*(N_horizon+m_steps),2*(N_horizon+m_steps)));
    blockzero.setZero();
    eye.setIdentity();
    for(int i=0; i!=s_stage; i++){
        Qtes << (alpha*eye + beta*(Pvu.transpose()*Pvu) + gamma*(Pzu.transpose()*Pzu)),
                (-gamma*(Pzu.transpose()*Ur[i])), (-gamma*(Ur[i].transpose()*Pzu)),
                (gamma*(Ur[i].transpose()*Ur[i]));
        (*Qaux) << Qtes, blockzero, blockzero, Qtes;
        Q.push_back((*Qaux));
    }

}


void MPCData::next(){
    stage++;
    if (stage >= s_stage){
        stage = 0;
    }
}

MatrixXr MPCData::getHessian(){
     return Q[stage];
}

VectorXr MPCData::getFvector(RobotState state, VectorXr footState, VectorXr dx_ref, VectorXr dy_ref){
    VectorXr x_k = state.getX();
    VectorXr y_k = state.getY();
    real_t xfc = footState(0);
    real_t yfc = footState(1);

    pk << beta*Pvu.transpose()*(Pvs*x_k-dx_ref) + gamma*Pzu.transpose()*(Pzs*x_k-Uc[stage]*xfc),
            -gamma*Ur[stage].transpose()*(Pzs*x_k-Uc[stage]*xfc),
            beta*Pvu.transpose()*(Pvs*y_k-dy_ref) + gamma*Pzu.transpose()*(Pzs*y_k-Uc[stage]*yfc),
            -gamma*Ur[stage].transpose()*(Pzs*y_k-Uc[stage]*yfc);

    return pk;

}



#ifdef _COMPILE_DEBUGGING_METHODS

void MPCData::printConstants(){
    using std::cout;
    using std::endl;

    cout << "Initialized with: " << endl;
    cout << "Alpha: " << alpha << ", ";
    cout << "Beta: " << beta << ", ";
    cout << "Gamma: " << gamma << ", ";

    cout << "Sampling period: " << T_s << ", ";
    cout << "Steps horizon: " << m_steps << ", ";
    cout << "Periods per step: " << s_stage << ", ";
    cout << "Control horizon: " << N_horizon << ", ";

    cout << "CoM Height: " << h_com << ", ";
    cout << "Gravity: " << g << ", ";
    cout << "Current stage: " << stage << endl;
}
#endif //_COMPILE_DEBUGGING_METHODS




}
